/********************** (C) COPYRIGHT 2007-2009 RAISONANCE ********************
* File Name          :  EarlTestingc
* Author             :  Clark Scheff
* Date First Issued  :  6/17/2009
* Description        :  Application for controling the iRobot Create for iAROC.
* Revision           :  666
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "EarlTesting.h"

/* Private defines -----------------------------------------------------------*/

// The following should be the minimal CircleOS version needed by your application
#define NEEDEDVERSION "V 3.7"
#define abs(a) ((0) <= (a) ? (a) : (-a))

const char Application_Name[8+1] = {"EarlTest"};      // Max 8 characters
u32 microSeconds = 0;
u32 oldPLL = 0;
CREATE_DATA cd;

const u8 *MarioSong = "smb:d=4,o=5,b=100:16e6,16e6,32p,8e6,16c6,8e6,8g6,8p,8g,8p,8c6,16p,8g,16p,8e,16p,8a,8b,16a#,8a,16g.,16e6,16g6,8a6,16f6,8g6,8e6,16c6,16d6,8b,16p,8c6,16p,8g,16p,8e,16p,8a,8b,16a#,8a,16g.,16e6,16g6,8a6,16f6,8g6,8e6,16c6,16d6,8b,8p,16g6,16f#6,16f6,16d#6,16p,16e6,16p,16g#,16a,16c6,16p,16a,16c6,16d6,8p,16g6,16f#6,16f6,16d#6,16p,16e6,16p,16c7,16p,16c7,16c7,p,16g6,16f#6,16f6,16d#6,16p,16e6,16p,16g#,16a,16c6,16p,16a,16c6,16d6,8p,16d#6,8p,16d6,8p,16c6";

GLOBALS globals;


/*******************************************************************************
* Function Name  : Application_Ini
* Description    : Initialization function of Circle_App. This function will
*                  be called only once by CircleOS.
* Input          : None
* Return         : MENU_CONTINUE_COMMAND
*******************************************************************************/
enum MENU_code Application_Ini(void)
    {
    // Ensure that the current OS version is recent enough
    if(strcmp(UTIL_GetVersion(), NEEDEDVERSION) < 0)
    {
//        return MsgVersion();
    }

    /* Set the clock to the maximum of 72 MHz */
    u8 oldPLL = UTIL_GetPll();
    UTIL_SetPll( SPEED_VERY_HIGH );
        
    // TODO: Write your application initialization function here.
    DRAW_Clear();
    DRAW_DisplayString(0, 100, "F :", 3);
    DRAW_DisplayString(0, 85, "RF:", 3);
    DRAW_DisplayString(0, 70, "RR:", 3);
    DRAW_DisplayString(0, 55, "D :", 3);
    DRAW_DisplayString(0, 40, "A :", 3);

    TIM1_Init();
    CREATE_Init();
    GLOBALS_Init();
    
    BUZZER_PlayMusic(MarioSong);
    
    return MENU_CONTINUE_COMMAND;
    }

/*******************************************************************************
* Function Name  : Application_Handler
* Description    : Management of the Circle_App.
*
* Input          : None
* Return         : MENU_CONTINUE
*******************************************************************************/
enum MENU_code Application_Handler(void)
    {
    u32 time;
    static u8 i;

    // TODO: Write your application handling here.


    // This routine will get called repeatedly by CircleOS, until we
    // return MENU_LEAVE

#if 1
    // If the button is pressed, the application is exited
    if(BUTTON_GetState() == BUTTON_PUSHED)
        {
        BUTTON_WaitForRelease();
        Create_Start(USART1, MODE_SAFE);
        UTIL_SetPll( oldPLL );
        return MENU_Quit();
        }
#endif
    updateSensors();
    if( TOUCHSCR_IsPressed() == 1 )
        followTheWall_grid();
    JoystickMovement();

    updateStatus();
    return MENU_CONTINUE;   // Returning MENU_LEAVE will quit to CircleOS
    }

/*******************************************************************************
* Function Name  : MsgVersion
* Description    : Display the current CircleOS version and the version needed
*                  exit to main menu after 4 secondes
*
* Input          : None
* Return         : MENU_REFRESH
*******************************************************************************/
static enum MENU_code MsgVersion(void)
   {
   int hh,mm,ss,ss2;
   
   DRAW_DisplayString(5,60,"CircleOS",17);
   DRAW_DisplayString(80,60,UTIL_GetVersion(),6);
   DRAW_DisplayString(5,34,NEEDEDVERSION,6);
   DRAW_DisplayString(50,34," required",12);
   
   RTC_GetTime( &hh, &mm, &ss);
   ss = ss + 4;                  // 4 secondes 
   ss = ss%60;
   
   do
        {
        RTC_GetTime( &hh, &mm, &ss2 );
        }
   while ( ss2 != ss );           // do while < 4 secondes
   
   DRAW_Clear();
   return MENU_REFRESH;
   }


/*******************************************************************************
* Function Name  : USART1_Init
* Description    : Initializes USART1 at 57,600bps for communicating with the 
*                  iRobot Create.
*
* Input          : None
* Return         : None
*******************************************************************************/
void USART1_Init( void )
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO | RCC_APB2Periph_USART1, ENABLE);

    GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);

    /* Configure USART1 Tx (PB.6) as alternate function push-pull */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    /* Configure USART1 Rx (PB.7) as input floating */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOB, &GPIO_InitStructure);


    USART_InitTypeDef USART_InitStructure;
    USART_InitStructure.USART_BaudRate = 57600;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
    USART_Init(USART1, &USART_InitStructure);
    
//    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    
    /* Enable the USART1 */
    USART_Cmd(USART1, ENABLE);
}


/*******************************************************************************
* Function Name  : TIM1_Init
* Description    : Initializes TIM1 to increment every 1usec and enables the  
*                  update interrupt.
*
* Input          : None
* Return         : None
*******************************************************************************/
void TIM1_Init( void )
{
    TIM_TimeBaseInitTypeDef TIM_InitStructure;

    /* Enable TIM1 Clock */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, DISABLE);

    /* Enable the Interrupt handler for TIM1_Update */
    UTIL_SetIrqHandler(0xA4, (u32)TIM1_Irq);
    UTIL_SetIrqHandler(0xD4, (u32)USART1_IRQHandler);
    NVIC_Configuration();

    RCC_ClocksTypeDef RCC_ClocksStatus;
    RCC_GetClocksFreq(&RCC_ClocksStatus);

    /* Setup timer to count in 1us increments */
    TIM_InitStructure.TIM_Period = 0xFFFF;
    TIM_InitStructure.TIM_Prescaler = RCC_ClocksStatus.SYSCLK_Frequency / 1000000;
    TIM_InitStructure.TIM_ClockDivision = TIM_CKD_DIV2;
    TIM_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_InitStructure.TIM_RepetitionCounter = 0;

    TIM_TimeBaseInit(TIM1, &TIM_InitStructure);
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE );
    TIM_Cmd(TIM1, ENABLE);
    TIM_Cmd(TIM2, DISABLE);  //disable TIM2 to avoid MEMS lockups (I hope)
    
    TIM1->CR1 |= CR1_CEN_Set;
}


/*******************************************************************************
* Function Name  : NVIC_Configuration
* Description    : Configure the nested vectored interrupt controller.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void NVIC_Configuration(void)
{
    NVIC_InitTypeDef NVIC_InitStructure;

    /* Enable the TIM1 Up event Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQChannel;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;
    NVIC_Init(&NVIC_InitStructure);
}


/*******************************************************************************
* Function Name  : CREATE_Init
* Description    : Calls USART1_Init() to setup the serial communication and 
*                  sens the create the startup commands to put it in full mode.
*
* Input          : None
* Return         : None
*******************************************************************************/
void CREATE_Init( void )
{
    USART1_Init();
    Create_Start(USART1, CMD_FULL);
}


/*******************************************************************************
* Function Name  : GLOBALS_Init
* Description    : Initializes the global variables to their defaults.
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
void GLOBALS_Init( void )
{
    globals.frontPing = 0;
    globals.rfPing = 0;
    globals.rrPing = 0;
    globals.leftVel = 0;
    globals.rightVel = 0;
    globals.distance = 0;
    globals.angle = 0;
}    


void JoystickMovement( void )
{
    char buffer[4];
    static u32 lastUpdate = 0;
    u16 dist;
    u16 angle;
    
    if(getTimeMicro() - lastUpdate < 1000000)
        return;
    switch( JOYSTICK_GetState() )
    {
        case JOYSTICK_UP:
            if( globals.frontPing <= 22 )
                return;
            Create_GetDistAngle(USART1, &dist, &angle);
            if( globals.frontPing < 81 )
            {
                u16 dist = (globals.frontPing - 20) * 10;
                UTIL_uint2str(buffer, globals.frontPing - 20, 3, 1);
                DRAW_DisplayString(0, 30, buffer, 3);

                UTIL_uint2str(buffer, (globals.frontPing - 20) * 10, 4, 1);
                DRAW_DisplayString(0, 15, buffer, 4);
 
                Create_DriveMillimeters(USART1, dist, 500);
            }
            else
                Create_DriveMillimeters(USART1, 610, 500);
            break;
        case JOYSTICK_DOWN:
            Create_GetDistAngle(USART1, &dist, &angle);
            Create_DriveMillimeters(USART1, -610, 500);
            break;
        case JOYSTICK_LEFT:
            Create_GetDistAngle(USART1, &dist, &angle);
            Create_TurnAngle(USART1, 90, 250);
            break;
        case JOYSTICK_RIGHT:
            Create_GetDistAngle(USART1, &dist, &angle);
            Create_TurnAngle(USART1, -90, 250);
            break;
        default:
            if(globals.leftVel == 0 && globals.rightVel == 0)
                return;
            globals.leftVel = 0;
            globals.rightVel = 0;
            Create_DriveDirect(USART1, globals.rightVel, globals.leftVel);
    }
    lastUpdate = getTimeMicro();
}

/*******************************************************************************
* Function Name  : updateStatus
* Description    : Displays useful information onto the LCD of the Primer 2
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
void updateStatus( void )
{
    char buffer[6];
    u32 lastReading = 0;

    DRAW_SetTextColor(RGB_RED);
    
    UTIL_uint2str(buffer, globals.frontPing, 3, 1);
    DRAW_DisplayString(20, 100, buffer, 3);

    UTIL_uint2str(buffer, globals.rfPing, 3, 1);
    DRAW_DisplayString(20, 85, buffer, 3);

    UTIL_uint2str(buffer, globals.rrPing, 3, 1);
    DRAW_DisplayString(20, 70, buffer, 3);
    
    UTIL_int2str(buffer, globals.distance, 5, 1);
    DRAW_DisplayString(20, 55, buffer, 6);

    UTIL_int2str(buffer, globals.angle, 5, 1);
    DRAW_DisplayString(20, 40, buffer, 6);
}


/*******************************************************************************
* Function Name  : TIM1_Irq
* Description    : TIM1 Update interrupt, called each time the timer overflows.
*                  microSeconds is incremented each time this happens, keeping
*                  track of usecs as a 32-bit value.
*
* Input          : None
* Return         : None
*******************************************************************************/
tHandler TIM1_Irq(void)
{
    TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    microSeconds += 0x00010000;
}


/*******************************************************************************
* Function Name  : getTimeMicro
* Description    : Returns the # of microseconds that have elapsed.
*               
*
* Input          : None
* Return         : None
*******************************************************************************/
u32 getTimeMicro( void )
{
    return microSeconds + TIM1->CNT;
}


void updateSensors( void )
{
    static u32 lastReading = 0;
    s16 dist;
    s16 angle;
    
    if( getTimeMicro() - lastReading < 15000 )
        return;
    
    u16 ping = PingPin(GPIOPing, FRONT_PING);
    globals.frontPingRaw = ping;
    globals.frontPing = (u16)convertTimeToCM(ping);
    
    ping = PingPin(GPIOPing, RF_PING);
    globals.rfPingRaw = ping;
    globals.rfPing = (u16)convertTimeToCM(ping);
    
    ping = PingPin(GPIOPing, RR_PING);
    globals.rrPingRaw = ping;
    globals.rrPing = (u16)convertTimeToCM(ping);
    
    if( Create_GetData(USART1, &cd) != 0 )
    {
        globals.distance += cd.distance;
        globals.angle += cd.angle;
    
        globals.angle = (s16)clampDegrees(globals.angle);
    
        globals.bumpers = cd.bumps_wheels;
    }
    lastReading = getTimeMicro();
}


void alignWithWall( void )
{
    updateSensors();
    
    if( globals.rfPingRaw > globals.rrPingRaw )
    {
        Create_DriveDirect(USART1, -20, 20);
        while( globals.rfPingRaw > globals.rrPingRaw )
            updateSensors();
        updateStatus();
        Create_Drive(USART1, 0, 0);
    }
    else if( globals.rrPingRaw > globals.rfPingRaw )
    {
        Create_DriveDirect(USART1, 20, -20);
        while( globals.rrPingRaw > globals.rfPingRaw )
            updateSensors();
        updateStatus();
        Create_Drive(USART1, 0, 0);
    }
}

void followTheWall( void )
{
    DRAW_DisplayString(0, 15, "               ", 15);
    updateSensors();
    if((globals.rfPing > 100 && globals.rrPing > 100) || globals.frontPing <= 25)
        return;
    else
        Create_Drive(USART1, 500, 0x7FFF);
    while( (globals.rfPing < 100 || globals.rrPing < 100) )
    {
        updateSensors();
        updateStatus();
        if( globals.frontPing <= 20 )
        {
            DRAW_DisplayString(0, 15, "STOPPED-WALL", 12);
            Create_Drive(USART1, 0, 0);
            return;
        }
        s16 diff = globals.rfPing - globals.rrPing;
        diff = abs(diff);
        if( (diff > 2) && (diff < 5) )
        {
            DRAW_DisplayString(0, 15, "ALIGNING    ", 12);
            alignWithWall();
        }
        Create_Drive(USART1, 500, 0x7FFF);
    }
    DRAW_DisplayString(0, 15, "OPENING SEEN", 12);
    Create_Drive(USART1, 0, 0);
}


void followTheWall_grid( void )
{
    u8 done = 0;
    u8 state = 0;
    u8 speed = 250;
    u8 turnSpeed = 50;
    static enum eMazeExplorerStates currState = STATE_IDLE;
    static enum eMazeExplorerStates prevState = STATE_IDLE;
    static enum eOrientation orientation = NORTH;
    
    globals.distance = 0;
    globals.angle = 0;
    DRAW_DisplayString(0, 15, "               ", 15);
    
    while( !done && !(BUTTON_GetState() == BUTTON_PUSHED))
    {
        DRAW_DisplayString(0, 15, "               ", 15);
        updateSensors();
        updateStatus();
        
        if( globals.frontPing > FRONT_OBSTACLE_DIST && 
            (globals.rfPing < SIDE_OBSTACLE_DIST && globals.rrPing < SIDE_OBSTACLE_DIST) ||
             prevState == STATE_ROTATE_CW )
        {
            if( prevState != STATE_ROTATE_CW)
            {
                alignWithWall();
                globals.angle = makeRightAngle(globals.angle);
            }
            globals.distance = 0;
            u16 dist = CELL_DISTANCE;
            if( globals.frontPing <= 106 )
                dist = (globals.frontPing - FRONT_OBSTACLE_DIST + 2) * 10;
            u16 travelled = goForward(dist, speed);
            if( travelled < dist )
                goForward(dist-travelled, speed/2);
            prevState = STATE_MOVE_FORWARD;
            currState = STATE_IDLE;
        }
        
        else if(globals.frontPing <= FRONT_OBSTACLE_DIST+2 && 
            globals.rfPing <= SIDE_OBSTACLE_DIST && globals.rrPing <= SIDE_OBSTACLE_DIST)
        {
            rotateCCW(90, turnSpeed);
            
            prevState = STATE_ROTATE_CCW;
            currState = STATE_IDLE;
        }
        
        else if(globals.rfPing > SIDE_NO_OBSTACLE_DIST && globals.rrPing > SIDE_NO_OBSTACLE_DIST)
        {
            rotateCW(90, turnSpeed);
            
            prevState = STATE_ROTATE_CW;
            currState = STATE_IDLE;
        }
        
//        u32 time = getTimeMicro();
//        while(getTimeMicro() - time < 1000000);
    }
}

s16 goForward(s16 distance, u16 speed)
{
    u16 dist;
    u16 startDist = globals.distance;
    
    Create_DriveDirect(USART1, speed, speed);
    DRAW_DisplayString(0, 15, "GOING FORWARD  ", 15);
    while( globals.distance-startDist < distance )//&& (globals.bumpers & 0x03 == 0))
    {
        updateSensors();
        updateStatus();
        if(globals.bumpers & 0x03)
        {
            break;
        }
    }
    
    Create_DriveDirect(USART1, 0, 0);
    return globals.distance-startDist;
}

s16 rotateCW(s16 angle, u16 speed)
{
    s16 currangle = globals.angle;
    s16 target_angle = makeRightAngle(clampDegrees(currangle - angle));

    DRAW_DisplayString(0, 15, "ROTATING CW    ", 15);
    Create_DriveDirect(USART1, -speed, speed);
    s16 diff;
    do
    {
        diff = globals.angle-target_angle;
        updateSensors();
        updateStatus();
    } while ( abs(diff) >= 1 );
    
    Create_DriveDirect(USART1, 0, 0);
    return globals.angle;
}


s16 rotateCCW(s16 angle, u16 speed)
{
    s16 currangle = globals.angle;
    s16 target_angle = makeRightAngle(clampDegrees(currangle + angle));

    DRAW_DisplayString(0, 15, "ROTATEING CCW  ", 15);
    Create_DriveDirect(USART1, speed, -speed);
    
    s16 diff;
    do
    {
        updateSensors();
        updateStatus();
        diff = abs((globals.angle-target_angle));
    } while ( diff >= 1 );
    
    Create_DriveDirect(USART1, 0, 0);
    return globals.angle;
}


u16 clampDegrees(s16 angle)
{
    if(angle > 360)
	angle -= 360;

    if(angle < 0)
	angle += 360;

    return (u16)angle;
}


u16 makeRightAngle(s16 angle)
{
    u8 modulus = 0;
    angle = clampDegrees(angle);
    
    modulus = angle % 90;

    if( modulus > 45 )
	angle = angle + (90 - modulus);
    else
	angle = angle - modulus;

    return angle;
}
